#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import numpy as np
import cv2
import rospy
from std_msgs.msg import Int32
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from ackermann_msgs.msg import AckermannDriveStamped
from geometry_msgs.msg import Twist
import sys

# 巡线模块
import wig_lane_dec

# 巡线图像查看模块1
import Capture_lane_image

#  巡线图像查看模块2——HSV
import HSV_Capture_lane_image

# 三标牌触发器识别
import detect3_blue_mask

# 第一套识别方案
import The_first_schemes_for_sign_detect as first

# 蓝色线段检测模块
import get_blue_line as blue_line

global msg, data  # 其中msg是小车巡线控制代码，data是发向激光雷达的控制代码

global angle_zero  # 小车直走时给的补偿角度


# 初始化的总函数
def init_global():
    # 初始化一些基本参数
    global msg, data  # 其中msg是小车巡线控制代码，data是发向激光雷达的控制代码
    global angle_zero  # 小车直走时给的补偿角度
    angle_zero = -6
    msg = AckermannDriveStamped()

    # 一些模块的初始化函数
    Capture_lane_image.init_lane_detect()
    HSV_Capture_lane_image.init_lane_detect()
    wig_lane_dec.init_lane_detect()
    detect3_blue_mask.init_detect()
    first.init_detect_portable()
    blue_line.init_detect()


# 前置摄像头回调函数
def front_camera_callback(data):
    print("get image")
    img = CvBridge().imgmsg_to_cv2(data, "bgr8")

    # 这里进行了图像的畸变矫正
    # img = Capture_lane_image.Straighten_the_camera(img)

    # 图像显示测试
    # show_image(img)

    # 进行巡线的调试1
    lane_detect_test(img)

    # 进行图片处理的调试2_HSV
    # Image_HSV_Processing(img)

    # # 图像预处理的调试
    # Image_preprocessing(img)

    # 三标牌触发器的调试
    # three_blue_mask(img)

    # 蓝色线条检测
    # find_blue_line(img)


# 主节点
def ros_node_init():
    global pub
    rospy.init_node('camera_cmd', anonymous=False)
    init_global()
    rospy.Subscriber("/usb_cam_1/image", Image, front_camera_callback, queue_size=1, buff_size=2 ** 24)
    pub = rospy.Publisher('/ackermann_cmd', AckermannDriveStamped, queue_size=1)
    rospy.spin()


# 显示图片的函数
def show_image(image):
    cv2.imshow('wig_show', image)
    cv2.waitKey(1)


# 图像预处理查看函数
def Image_preprocessing(img):
    camera_get = img
    # 进行透视操作
    warp_img = Capture_lane_image.warp_image(camera_get)
    # 进行二值化操作
    bina_img = Capture_lane_image.Binary_image(warp_img)
    cv2.imshow('lane_detect_pic', camera_get)
    cv2.imshow('warp_img', warp_img)
    cv2.imshow('bina_img', bina_img)

    key_wig = cv2.waitKey(1)
    if (key_wig & 0xFF) == ord('q'):
        print("exit")

    elif (key_wig & 0xFF) == ord('s'):
        cv2.imwrite(
            # 这里的保存路径不能有英文
            "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_original.png",
            camera_get,
            None
        )
        print("the original_image has been saved")
    elif (key_wig & 0xFF) == ord('w'):
        cv2.imwrite(
            # 这里的保存路径不能有英文
            "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_warp.png",
            warp_img,
            None
        )
        print("The image of warp has been saved")

    elif (key_wig & 0xFF) == ord('b'):
        cv2.imwrite(
            # 这里的保存路径不能有英文
            "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_bina.png",
            bina_img,
            None
        )
        print("The image of bina has been saved")

    # 巡线函数


# 图像预处理查看函数2HSV
def Image_HSV_Processing(img):
    # 进行透视操作
    warp_img = HSV_Capture_lane_image.warp_image(img)
    # 进行二值化操作
    bina_img = HSV_Capture_lane_image.Binary_image(warp_img)

    cv2.imshow('lane_detect_pic', img)
    cv2.imshow('warp_img', warp_img)
    cv2.imshow('white', bina_img)

    key_wig = cv2.waitKey(1)
    if (key_wig & 0xFF) == ord('q'):
        print("exit")

    elif (key_wig & 0xFF) == ord('s'):
        cv2.imwrite(
            # 这里的保存路径不能有英文
            "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_original.png",
            img,
            None
        )
        print("the original_image has been saved")
    elif (key_wig & 0xFF) == ord('w'):
        cv2.imwrite(
            # 这里的保存路径不能有英文
            "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_warp.png",
            warp_img,
            None
        )
        print("The image of warp has been saved")

    elif (key_wig & 0xFF) == ord('b'):
        cv2.imwrite(
            # 这里的保存路径不能有英文
            "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_bina.png",
            bina_img,
            None
        )
        print("The image of bina has been saved")


# 巡线测试函数
def lane_detect_test(img):
    # 对图片进行处理
    warp_img = wig_lane_dec.warp_image(img)
    bina_img = wig_lane_dec.Binary_image(warp_img)

    # 获得边界
    left_bound_445, right_bound_445 = wig_lane_dec.boundary_detect(bina_img)
    # 获得巡线模式
    dec_line_mode = wig_lane_dec.get_line_mode(left_bound_445, right_bound_445)
    # 获得角度和倒车标志
    angle, pos_flag = wig_lane_dec.get_angle(left_bound_445, right_bound_445, dec_line_mode, bina_img)

    print('angle_original:', angle)

    # 给出最终角度
    msg.drive.steering_angle = angle + angle_zero

    print('angle:', msg.drive.steering_angle)

    # 倒车判断
    if pos_flag == -1:
        # 倒车的话，将角度转为负数
        msg.drive.steering_angle = (-msg.drive.steering_angle) * 0.5
        # 速度控制为20
        msg.drive.speed = 20 * pos_flag
        # 发布消息
        pub.publish(msg)
        # 倒车最小持续时间为0.5s
        rospy.sleep(0.5)

    # 没有倒车就正常行驶
    elif pos_flag == 1:

        msg.drive.speed = 30 * pos_flag
        pub.publish(msg)

    # 出错就减慢速度
    else:
        msg.drive.speed = 10
        pub.publish(msg)

    cv2.imshow('bina_img', bina_img)

    # 等待10ms看有没有按键按下，读取指令
    key_wig = cv2.waitKey(1)

    # 这里还设置了保存指令
    if (key_wig & 0xFF) == ord('s'):
        cv2.imwrite(
            "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/bina_img.png",
            bina_img,
            None
        )
        print("The image of bina_img has been saved")


# 三圆形标牌触发器识别函数
def three_blue_mask(img):
    # 进行三个标志的判断
    detect3_blue_mask.Judge_functions_3_circle(img)
    # 等待10ms看有没有按键按下，读取指令
    # first.Judge_execute_functions(img)
    cv2.waitKey(1)


# 检测蓝色线条的函数
def find_blue_line(img):
    points11, points22, line = blue_line.find_blue_line_give_points(img)

    line_img = blue_line.draw_line_2(img, line)

    print("points:", points11, points22)

    cv2.imshow('original', img)
    cv2.imshow('draw_line:', line_img)

    # 等待10ms看有没有按键按下，读取指令
    key_wig = cv2.waitKey(1)

    # 这里设置了退出的指令
    if (key_wig & 0xFF) == ord('q'):
        print("Exit the programme")


if __name__ == "__main__":
    ros_node_init()

